#include "m3508.h"
#include "string.h"

#define M3508_CONTROL_ID_GROUP1 0x200
#define M3508_CONTROL_ID_GROUP2 0x1ff

M3508_FeedbackTypeDef m3508_feedback;
PID_TypeDef m3508_rpm_pid; // PID控制器实例
float m3508_reducer_rpm_goal = 0.0f; // 减速器转速目标值


/**
 * @brief  启动M3508电机的CAN通信，需要提前配置好CAN接口的波特率等物理量并进行
 * 初始化，该动作已由CubeMX自动生成的代码完成。该函数会配置CAN过滤器，接收邮箱和中断等。
 * @note   该函数会在MXMX_CANx_Init()之后调用，确保CAN模块已经初始化完成。
 * @param  None
 * @retval None
 */
void M3508_CAN_Start(void)
{
    CAN_FilterTypeDef m3508_filter;
    m3508_filter.FilterActivation = CAN_FILTER_ENABLE; // 使能过滤器
    m3508_filter.FilterBank = 14; // 选择过滤器组 (CAN2选14-27, CAN1选0-13)
    m3508_filter.FilterFIFOAssignment = CAN_FILTER_FIFO0; // 选择接收邮箱
    m3508_filter.FilterScale = CAN_FILTERSCALE_16BIT; // 设置过滤器规模
    m3508_filter.FilterMode = CAN_FILTERMODE_IDMASK; // 设置过滤器模式
    m3508_filter.FilterIdHigh = 0x40; // 设置过滤器ID高位 0x40
    m3508_filter.FilterIdLow = 0x00; // 设置过滤器ID低位 0x00
    m3508_filter.FilterMaskIdHigh = 0xFE; // 设置过滤器掩码高位 0xFE
    m3508_filter.FilterMaskIdLow = 0x18; // 设置过滤器掩码低位 0x18
    m3508_filter.SlaveStartFilterBank = 14; // 单CAN此参数无意义，设置can1(0-13)和can2(14-27)分别得到一半的filter
    // 只接受标准ID为0x200 - 0x20F的数据帧报文
    HAL_CAN_ConfigFilter(&M3508_CAN_HANDLE, &m3508_filter); // 配置过滤器

    HAL_CAN_Start(&M3508_CAN_HANDLE); // 启动CAN模块
    HAL_CAN_ActivateNotification(&M3508_CAN_HANDLE, CAN_IT_RX_FIFO0_MSG_PENDING); // 激活RX_FIFO0接收中断
}

/**
 * @brief  设置单个M3508电机的控制电流值
 * @param  hcan: 指向CAN句柄的指针
 * @param  motor_id: 电机ID (1-8)
 * @param  goal_current: 控制电流值 (范围：-16384 ~ 16384, 对应于-20A ~ 20A)
 * @retval CAN使用的发送邮箱号 (一般没啥用)
 */
uint32_t M3508_SetCurrent(CAN_HandleTypeDef *hcan, uint8_t motor_id, int16_t goal_current)
{
    // 设置发送数据的CAN报文头
    CAN_TxHeaderTypeDef tx_header;
    tx_header.IDE = CAN_ID_STD; // 报文ID为标准ID
    tx_header.RTR = CAN_RTR_DATA; // 数据帧
    tx_header.DLC = 8;  // 数据长度为8字节
    tx_header.StdId = motor_id > 4 ? M3508_CONTROL_ID_GROUP2 : M3508_CONTROL_ID_GROUP1; // 设置报文ID

    // 向数据缓冲区中写入指定电机的控制电流值
    if (goal_current > 16384) goal_current = 16384; // 限制最大值
    if (goal_current < -16384) goal_current = -16384; // 限制最小值
    motor_id = motor_id > 4 ? motor_id - 5 : motor_id - 1; // 将电机ID转换为0-3的范围
    uint8_t tx_data[8] = {0}; // 数据缓冲区全部清零
    tx_data[motor_id << 1] =  goal_current >> 8 & 0xFF; // 高字节
    tx_data[motor_id << 1 | 1] = goal_current & 0xFF; // 低字节

    uint32_t tx_mailbox; // CAN模块自动使用的发送邮箱号
    HAL_CAN_AddTxMessage(hcan, &tx_header, tx_data, &tx_mailbox);

    return tx_mailbox; // 返回发送邮箱号
}


void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
    if (hcan->Instance == M3508_CAN_HANDLE.Instance) // 确保是M3508 CAN实例
    {
        CAN_RxHeaderTypeDef rx_header;
        uint8_t rx_data[8] = {0};
        HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data); // 获取接收到的CAN报文   

        m3508_feedback.id = rx_header.StdId - 0x200; // 获取电机ID
        m3508_feedback.angle = rx_data[0] << 8 | rx_data[1]; // 获取电机转子角度
        m3508_feedback.rpm = rx_data[2] << 8 | rx_data[3]; // 获取电机转速
        m3508_feedback.actual_current = rx_data[4] << 8 | rx_data[5]; // 获取实际转矩电流
        m3508_feedback.temperature = rx_data[6]; // 获取电机温度

        //osThreadFlagsSet(m3508CtrlTaskHandle, 0x01U);
    }
}

/**
 * @brief  初始化M3508减速器输出转速控制PID控制器
 * @param  Kp: 比例系数
 * @param  Ki: 积分系数
 * @param  Kd: 微分系数
 * @param  deadband: 死区范围
 * @param  integral_limit: 积分限幅值
 * @retval None
 */
void M3508_ReducerRpmControl_Init(float Kp, float Ki, float Kd, float deadband, float integral_limit)
{
    m3508_rpm_pid.mode = POSITION_PID;
    m3508_rpm_pid.Kp = Kp;
    m3508_rpm_pid.Ki = Ki;
    m3508_rpm_pid.Kd = Kd;
    m3508_rpm_pid.deadband = deadband;
    m3508_rpm_pid.output_limit = 16384; // 最大输出电流，正负值
    m3508_rpm_pid.integral_limit = integral_limit; // 积分限幅幅
    memset(m3508_rpm_pid.error, 0, sizeof(m3508_rpm_pid.error)); // 清零误差数组
    m3508_rpm_pid.delta_out[LAST] = 0.0f; // 清零增量输出
    m3508_rpm_pid.goal_last = 0.0f; // 清零上次目标值

    print_log("M3508 Reducer RPM Control Init: Kp=%.2f, Ki=%.2f, Kd=%.2f, deadband=%.2f, integral_limit=%.2f\n",
           m3508_rpm_pid.Kp, m3508_rpm_pid.Ki, m3508_rpm_pid.Kd, m3508_rpm_pid.deadband, m3508_rpm_pid.integral_limit);
}

/**
 * @brief  设置M3508减速器目标转速
 * @param  goal_rpm: 目标减速器转速 (单位：RPM)
 * @retval None
 */
void M3508_ReducerRpmControl_SetGoal(float goal_rpm)
{
    m3508_reducer_rpm_goal = goal_rpm; // 设置目标转速
    print_log("M3508 Reducer RPM Goal Set: %.2f RPM\n", m3508_reducer_rpm_goal);
}

/**
 * @brief  M3508减速器转速控制函数
 * @param  feedback: 电机反馈数据结构体
 * @param  goal_rpm: 目标减速器转速 (单位：RPM)
 * @retval 返回目标电流值 (范围：-16384 ~ 16384, 对应于-20A ~ 20A)
 */
int16_t M3508_ReducerRpm_Control(M3508_FeedbackTypeDef feedback)
{
    float goal_rpm = m3508_reducer_rpm_goal * 3591.0f / 187.0f; // 转换为电机转速
    // if (ABS(goal_rpm  - m3508_rpm_pid.goal_last) > 200.0f) // 限制目标转速变化率
    // {
    //     goal_rpm = goal_rpm > m3508_rpm_pid.goal_last ? m3508_rpm_pid.goal_last + 400.0f : m3508_rpm_pid.goal_last - 400.0f;
    //     m3508_rpm_pid.goal_last = goal_rpm; // 更新上次目标值
    // }
    float goal_current = PID_Calculate(&m3508_rpm_pid, (float)feedback.rpm, goal_rpm); // 计算PID输出

    return (int16_t)goal_current; // 返回目标电流值
}
